#include "kf_ll_yaw.h"

namespace auto_ros
{
namespace control
{
KfLLYaw::KfLLYaw(std::string kf_vehicle_yaml_path_name, double rate)
	: KfVehicleBase(kf_vehicle_yaml_path_name, rate)
{
	mass_ = kf_yaml_["mass"].as<double>();
	cf_ = kf_yaml_["cf"].as<double>();
	cr_ = kf_yaml_["cr"].as<double>();
	lf_ = kf_yaml_["lf"].as<double>();
	lr_ = kf_yaml_["lr"].as<double>();
	iz_ = kf_yaml_["iz"].as<double>();

	con_ptr_ = std::make_shared<ContinuousLinearSys>(6, 2, 0, 3);
	speed_threshold_ = 1.0;
	c_yaw_rate_ = -lf_ / 1.0 * cf_ + lr_ / 1.0 * cr_;
	c_beta_ = -(cf_ + cr_);
	c_delta_ = cf_;

	d_yaw_rate_ = -lf_ * lf_ / 1.0 * cf_ - lr_ * lr_ / 1.0 * cr_;
	d_delta_ = lf_ * cf_;
	d_beta_ = -lf_ * cf_ + lr_ * cr_;
	//init speed is 1.0m/s
	Eigen::MatrixXd state_matrix(con_ptr_->state_dim(), con_ptr_->state_dim());
	state_matrix << 0, 0, 0, 1, 0, 0,
		0, c_beta_ / mass_ / 1.0, c_yaw_rate_ / mass_ - 1.0, 0, 1, 0,
		0, d_beta_ / iz_ / 1.0, d_yaw_rate_ / iz_, 0, 0, 1,
		0, 0, 0, 0, 0, 0,
		0, 0, 0, 0, 0, 0,
		0, 0, 0, 0, 0, 0;
	con_ptr_->set_state_matrix(state_matrix);
	Eigen::MatrixXd control_matrix(con_ptr_->state_dim(), con_ptr_->control_dim());
	control_matrix << 0, 1,
		c_delta_ / mass_, 1,
		d_delta_ / iz_, 0,
		0, 0,
		0, 0,
		0, 0;
	con_ptr_->set_control_matrix(control_matrix);
	Eigen::MatrixXd out_state_matrix(con_ptr_->out_dim(), con_ptr_->state_dim());
	out_state_matrix << 1, 0, 0, 0, 0, 0,
		0, 1, 0, 0, 0, 0,
		0, 0, 1, 0, 0, 0;

	con_ptr_->set_out_state_matrix(out_state_matrix);
	dis_ptr_ = con_ptr_->discretization(1 / rate_, "tustin");

	Eigen::MatrixXd sys_covariance(con_ptr_->state_dim(), con_ptr_->state_dim());
	sys_covariance << kf_yaml_["std_vx"].as<double>(), 0, 0, 0, 0, 0,
		0, kf_yaml_["std_vy"].as<double>(), 0, 0, 0, 0,
		0, 0, kf_yaml_["std_yaw_rate"].as<double>(), 0, 0, 0,
		0, 0, 0, kf_yaml_["std_ax"].as<double>(), 0, 0,
		0, 0, 0, 0, kf_yaml_["std_ay"].as<double>(), 0,
		0, 0, 0, 0, 0, kf_yaml_["std_yaw_acc"].as<double>();
	Eigen::MatrixXd measure_covariance(con_ptr_->out_dim(), con_ptr_->out_dim());
	measure_covariance << kf_yaml_["std_vx_m"].as<double>(), 0, 0,
		0, kf_yaml_["std_vy_m"].as<double>(), 0,
		0, 0, kf_yaml_["std_yaw_rate_m"].as<double>();
	kf_ptr_ = std::make_shared<KF>(dis_ptr_, sys_covariance, measure_covariance);
	reset();
}
bool KfLLYaw::speed_update_sys(double speed)
{
	if (std::abs(speed) < speed_threshold_)
	{
		if (speed >= 0)
			speed = speed_threshold_;
		else
			speed = -speed_threshold_;
	}
	speed_ = speed;
	c_yaw_rate_ = -lf_ / speed * cf_ + lr_ / speed * cr_;
	d_yaw_rate_ = -lf_ * lf_ / speed * cf_ - lr_ * lr_ / speed * cr_;
	Eigen::MatrixXd state_matrix(con_ptr_->state_dim(), con_ptr_->state_dim());
	state_matrix << 0, 0, 0, 1, 0, 0,
		0, c_beta_ / mass_ / speed, c_yaw_rate_ / mass_ - speed, 0, 1, 0,
		0, d_beta_ / iz_ / speed, d_yaw_rate_ / iz_, 0, 0, 1,
		0, 0, 0, 0, 0, 0,
		0, 0, 0, 0, 0, 0,
		0, 0, 0, 0, 0, 0;
	if (con_ptr_->set_state_matrix(state_matrix) == false)
	{
		std::cout << "\033[31m Error:speed_update_sys failed,con_ptr_->set_state_matrix failed \033[0m" << std::endl;
		return false;
	}
	Eigen::MatrixXd control_matrix(con_ptr_->state_dim(), con_ptr_->control_dim());
	control_matrix << 0, 1,
		c_delta_ / mass_, 0,
		d_delta_ / iz_, 0,
		0, 0,
		0, 0,
		0, 0;
	if (con_ptr_->set_control_matrix(control_matrix) == false)
	{
		std::cout << "\033[31m Error:speed_update_sys failed,con_ptr_->set_control_matrix failed \033[0m" << std::endl;
		return false;
	}
	if (con_ptr_->discrete(dis_ptr_, 1 / rate_, "tustin") == false)
	{
		std::cout << "\033[31m Error:speed_update_sys failed,con_ptr_->discrete failed \033[0m" << std::endl;
		return false;
	}
	return true;
}
bool KfLLYaw::step(Eigen::VectorXd &control_last, Eigen::VectorXd &y, double speed)
{

	if (speed_update_sys(speed) == false)
	{
		std::cout << "\033[31m  KfLLYaw::step failed,speed_update_sys failed \033[0m" << std::endl;
		return false;
	}
	if (kf_ptr_->step(control_last, y) == false)
	{
		std::cout << "\033[31m  KfLLYaw::step failed,kf_ptr_->ste failed \033[0m" << std::endl;
		return false;
	}
	/*if (std::abs(speed) < speed_threshold_)
	{
		Eigen::VectorXd state_esti(6);
		state_esti << y(0), y(1), y(2), y(3), 0, 0;
		Eigen::MatrixXd temp_state_cov_esti(6, 6);
		temp_state_cov_esti << 0.01, 0, 0, 0, 0, 0,
			0, 0.01, 0, 0, 0, 0,
			0, 0, 0.01, 0, 0, 0,
			0, 0, 0, 0.01, 0, 0,
			0, 0, 0, 0, 0.01, 0,
			0, 0, 0, 0, 0, 0.01;
		if (kf_ptr_->reset(state_esti, temp_state_cov_esti) == false)
		{
			std::cout << "\033[31m  KfLLYaw::reset failed,kf_ptr_->reset failed \033[0m" << std::endl;
			return false;
		}
}*/
}
bool KfLLYaw::reset(Eigen::VectorXd state_esti, Eigen::MatrixXd state_cov_esti)
{
	if (kf_ptr_->reset(state_esti, state_cov_esti) == false)
	{
		std::cout << "\033[31m  KfLLYaw::reset failed,kf_ptr_->reset failed \033[0m" << std::endl;
		return false;
	}
	return true;
}

bool KfLLYaw::reset()
{
	int state_dim = con_ptr_->state_dim();
	Eigen::VectorXd state_esti = Eigen::VectorXd::Zero(state_dim);
	Eigen::MatrixXd state_cov_esti = Eigen::MatrixXd::Zero(state_dim, state_dim);
	if (kf_ptr_->reset(state_esti, state_cov_esti) == false)
	{
		std::cout << "\033[31m  KfLLYaw::reset failed,kf_ptr_->reset failed \033[0m" << std::endl;
		return false;
	}
	return true;
}
double KfLLYaw::mass()
{
	return mass_;
}
double KfLLYaw::lf()
{
	return lf_;
}
double KfLLYaw::lr()
{
	return lr_;
}
double KfLLYaw::cf()
{
	return cf_;
}
double KfLLYaw::cr()
{
	return cr_;
}
double KfLLYaw::iz()
{
	return iz_;
}

double KfLLYaw::speed_threshold()
{
	return speed_threshold_;
}
Eigen::VectorXd KfLLYaw::state_esti()
{
	return kf_ptr_->state_esti();
}
Eigen::MatrixXd KfLLYaw::state_cov_esti()
{
	return kf_ptr_->state_cov_esti();
}
} // namespace control
} // namespace auto_ros
